/*
 * CommunicationMan.cpp
 *
 *  Created on: Apr 2, 2011
 *      Author: Anton
 */

#include <motion/CommunicationMan.h>

int convertAngle(int angle, int potLow, int potHigh, int angLow, int angHigh) {
	double m = ((double)potHigh - (double)potLow) / ((double)angHigh - (double)angLow);
	double b = (double)potLow - (double)angLow * m;
	return (int)(m * (double)angle + b);
}

CommunicationMan::CommunicationMan(Robot* ST, char* ip, int port) {

rbe=ST;
//Set the IP address of the host here. (sbRIO)
host= (struct hostent *) gethostbyname(ip);
//If we can't create a socket, error is known. 
if ((sock = socket(AF_INET, SOCK_DGRAM, 0)) == -1)
{
perror("socket");
exit(1);
}
//Creates access to port 
server_addr.sin_family = AF_INET;
server_addr.sin_port = htons(port);
server_addr.sin_addr = *((struct in_addr *)host->h_addr);
bzero(&(server_addr.sin_zero),8);

}

CommunicationMan::~CommunicationMan() {
}

//sends a char[] through the socket. 
void CommunicationMan::sendString(const char *send_data){
    sendto(sock, send_data, strlen(send_data), 0,(struct sockaddr *)&server_addr, sizeof(struct sockaddr));
}

//Sends the current robot's angles using the socket we created when robot was initialized.
void CommunicationMan::sendAngles(){
	char a[1024];
	char b[1024];
	
	send_data[0] = '\0';
	a[0]='\0';
	b[0]='\0';
	char id[5]="Boob";
	memcpy(a, &id,sizeof(char)*5) ;
	
//FL2	
	short aa = convertAngle(rbe->legs[0].links[1].getQ(), 2000, 12000, -30, 60);
	memcpy(b,&aa,sizeof(short));
	strcat(a,b);
//FL3
	aa = convertAngle(rbe->legs[0].links[2].getQ(), 1000, 22000, -80, 80);
	memcpy(b,&aa,sizeof(short));
	strcat(a,b);
//FR2
	aa = convertAngle(rbe->legs[1].links[1].getQ(), 4000, 15000, -30, 60);
	memcpy(b,&aa,sizeof(short));
	strcat(a,b);
//FR3
	aa = convertAngle(rbe->legs[1].links[2].getQ(), 2000, 20500, -80, 80);
	memcpy(b,&aa,sizeof(short));
	strcat(a,b);
//RL2
	aa = convertAngle(rbe->legs[2].links[1].getQ(), 9000, 18500, -60, 50);
	memcpy(b,&aa,sizeof(short));
	strcat(a,b);
//RL3
	aa = convertAngle(rbe->legs[2].links[2].getQ(), 7000, 21000, 25, 125);
	memcpy(b,&aa,sizeof(short));
	strcat(a,b);
//RR2
	aa = convertAngle(rbe->legs[3].links[1].getQ(), 7500, 18000, -60, 50);
	memcpy(b,&aa,sizeof(short));
	strcat(a,b);
//RR3
	aa = convertAngle(rbe->legs[3].links[2].getQ(), 17000, 24000, 25, 125);
	memcpy(b,&aa,sizeof(short));
	strcat(a,b);
//FL1
	int aaa = convertAngle(rbe->legs[0].links[0].getQ(), -52000, 52000, -20, 20);
	memcpy(b,&aaa,sizeof(int));
	strcat(a,b);
//FR1
	aaa = convertAngle(rbe->legs[0].links[1].getQ(), -52000, 52000, -20, 20);
	memcpy(b,&aaa,sizeof(int));
	strcat(a,b);
//RL1
	aaa = convertAngle(rbe->legs[0].links[2].getQ(), -52000, 52000, -20, 20);
	memcpy(b,&aaa,sizeof(int));
	strcat(a,b);
//RR1
	aaa = convertAngle(rbe->legs[0].links[3].getQ(), -52000, 52000, -20, 20);
	memcpy(b,&aaa,sizeof(int));
	strcat(a,b);	

	strcat(send_data,a);
	sendto(sock, send_data, strlen(send_data), 0,(struct sockaddr *)&server_addr, sizeof(struct sockaddr));
	
}

